#include "plBullet.h"
#include "plbtaobindings.h"
#include "../core/ObjectFactory.h"

RegisterCppObj<PlBullet> g_register_plbullet_object;

PlRigidBody::PlRigidBody(PlBullet* world, int slotId)
{
	_slotid = slotId;
	_world = world;
	_stub = new GeneroStub<PlRigidBody>;
	_stub->refct = 1; // ourself
	_stub->target = this;
	//_btBody = new btRigidBody();
	_theBody = NULL;
	_shape = NULL;
	_inWorld = false;
}

PlRigidBody::~PlRigidBody()
{
	// uh stuff?
	removeFromWorld();
	std::list<PlConstraint*>::iterator i;
	for(i = _constraints.begin(); i != _constraints.end(); ++i)
	{
		if(*i != NULL)
			(*i)->detachBody(this);
	}
	_stub->target = NULL;
	--_stub->refct;
	if(_stub->refct <= 0)
		delete _stub;
	delete _theBody;
	if(_shape)
		_shape->decrRefCt();
	
}

int PlRigidBody::getSlotId()
{
	return _slotid;
}

GeneroStub<PlRigidBody>* PlRigidBody::getStub()
{
	return _stub;
}

btRigidBody* PlRigidBody::getBtBody()
{
	return _theBody;
}
	
void PlRigidBody::addConstraint(PlConstraint* constr)
{
	_constraints.push_back(constr);
}

void PlRigidBody::removeConstraint(PlConstraint* constr)
{
	_constraints.remove(constr);
	/*
	std::list<PlConstraint*>::iterator i;
	for(i = _constraints.begin(); i != _constraints.end();)
	{
		if(*i == constr)
			i = _constraints.erase(i);
		else
			++i;
	}*/
}

void PlRigidBody::setShape(PlShape* shp)
{
	if(_shape)
		_shape->decrRefCt();
	_shape = shp;
	_shape->incrRefCt();
}

/* other stuff */
void PlRigidBody::addToWorld()
{
	if(_theBody && _world && !_inWorld)
	{
		_world->getWorld()->addRigidBody(_theBody);
		_inWorld = true;
	}
	// eh
}

void PlRigidBody::removeFromWorld()
{
	if(_theBody && _world && _inWorld)
	{
		// remove any constraints referencing us from the world
		std::list<PlConstraint*>::iterator i;
		for(i = _constraints.begin(); i != _constraints.end(); ++i)
		{
			if(*i != NULL)
				(*i)->removeFromWorld();
		}
		_world->getWorld()->removeRigidBody(_theBody);
		_inWorld = false;
	}
	// eh
}

void PlRigidBody::setMass(double mass)
{
	_mass = (float)mass;
	if(_theBody)
		_theBody->setMassProps(_mass, _inertia);
}

void PlRigidBody::setInertia(btVector3& inertia)
{
	_inertia = inertia;
	if(_theBody)
		_theBody->setMassProps(_mass, _inertia);
}

void PlRigidBody::setPosition(btVector3& pos)
{
	_pos = pos;
    if(_theBody)
    {
            btTransform tr = _theBody->getWorldTransform();
            tr.setOrigin(_pos);
            _theBody->setWorldTransform(tr);
			// can we just do
			// _theBody->getWorldTransform().setOrigin(_pos)
    }
}

void PlRigidBody::setRotation(btQuaternion& rot)
{
	_rot = rot;
    if(_theBody)
    {
            btTransform tr = _theBody->getWorldTransform();
            tr.setRotation(_rot);
            _theBody->setWorldTransform(tr);
			// can we just do
			// _theBody->getWorldTransform().setRotation(_rot)
    }
}

void PlRigidBody::setLinearVelocity(const btVector3& vel)
{
        if(_theBody)
        {
                _theBody->setLinearVelocity(vel);
        }
}

void PlRigidBody::setAngularVelocity(const btVector3& rot)
{
        if(_theBody)
        {
                _theBody->setAngularVelocity(rot);
        }
}

btVector3& PlRigidBody::getPosition()
{
        if(_ms)
        {
                _ms->getWorldTransform(_tempTrans);
                _pos = _tempTrans.getOrigin();
        }

        return _pos;
}

btQuaternion& PlRigidBody::getRotation()
{
        if(_ms)
        {
                _ms->getWorldTransform(_tempTrans);
                _rot = _tempTrans.getRotation();
        }

        return _rot;
}

btVector3 PlRigidBody::getLinearVelocity()
{
	btVector3 bla(0,0,0);
	return bla;
}

btVector3 PlRigidBody::getAngularVelocity()
{
	btVector3 bla(0,0,0);
	return bla;
}

void PlRigidBody::applyGlobalForce(const btVector3& pos, const btVector3& force)
{
        if(_theBody)
                _theBody->applyForce(force, pos);
}

void PlRigidBody::applyLocalForce(const btVector3& pos, const btVector3& force)
{
        if(_theBody)
                _theBody->applyForce(localVecToWorld(force), localVecToWorld(pos));
}

void PlRigidBody::applyGlobalTorque(const btVector3& tq)
{
        if(_theBody)
                _theBody->applyTorque(tq);
}

void PlRigidBody::applyLocalTorque(const btVector3& tq)
{
        if(_theBody)
                _theBody->applyTorque(localVecToWorld(tq));
}

btVector3 PlRigidBody::localPosToWorld(const btVector3& pos)
{
        if(_theBody)
        {
                return _theBody->getWorldTransform() * pos;
        }
        else
                return btVector3(0,0,0);
}

btVector3 PlRigidBody::worldPosToLocal(const btVector3& pos)
{
        if(_theBody)
        {
                return _theBody->getWorldTransform().inverse() * pos;
        }
        else
                return btVector3(0,0,0);
}

btVector3 PlRigidBody::localVecToWorld(const btVector3& vec)
{
        if(_theBody)
        {
                _tempTrans = _theBody->getWorldTransform();
                _tempTrans.setOrigin(btVector3(0,0,0));
                return _tempTrans * vec;
        }
        else
                return btVector3(0,0,0);
}
btVector3 PlRigidBody::worldVecToLocal(const btVector3& vec)
{
        if(_theBody)  
        {
                _tempTrans.setRotation(_theBody->getWorldTransform().getRotation().inverse());
                _tempTrans.setOrigin(btVector3(0,0,0));
                return _tempTrans * vec;
        }
        else
                return btVector3(0,0,0);
}

void PlRigidBody::autoCalcInertia()
{
	if(_shape)
		_shape->getBtShape()->calculateLocalInertia(_mass, _inertia);
}

void PlRigidBody::finalizeBody()
{
        btDiscreteDynamicsWorld* pw = _world->getWorld();
		btCollisionShape* cs = _shape->getBtShape();

                _tempTrans.setOrigin(_pos);
                _tempTrans.setRotation(_rot);
                _ms = new btDefaultMotionState(_tempTrans);

                btRigidBody::btRigidBodyConstructionInfo tempCI(_mass, _ms, cs, _inertia);

                _theBody = new btRigidBody(tempCI);

				addToWorld();
                //pw->addRigidBody(_theBody);

				// deal with this later
                _theBody->setActivationState(DISABLE_DEACTIVATION);
}

PlConstraint::PlConstraint(PlBullet* world, int slotId)
{
	_world = world;
	_slotid = slotId;
	_stub = new GeneroStub<PlConstraint>;
	_stub->refct = 1;
	_stub->target = this;
	_constraint = NULL;
	_conType = CON_NONE;
	for(int i = 0; i < PL_MAX_CONSTRAINT_BODIES; ++i)
		_bodies[i] = NULL;
	_collidable = false;
	_inWorld = false;
}

PlConstraint::~PlConstraint()
{
	removeFromWorld();
	detachAllBodies();
	_stub->target = NULL;
	--_stub->refct;
	if(_stub->refct <= 0)
		delete _stub;
	delete _constraint;
}

btTypedConstraint* PlConstraint::getBtConstraint()
{
	return _constraint;
}

int PlConstraint::getSlotId()
{
	return _slotid;
}

GeneroStub<PlConstraint>* PlConstraint::getStub()
{
	return _stub;
}

void PlConstraint::detachAllBodies()
{
	for(int i = 0; i < PL_MAX_CONSTRAINT_BODIES; ++i)
	{
		if(_bodies[i])
			_bodies[i]->removeConstraint(this);
		_bodies[i] = NULL;
	}
}

void PlConstraint::detachBody(PlRigidBody* body)
{
	if(body == NULL)
	{
		LOG(logWARNING) << "Tried to detach null body from constraint (no effect).";
		return;
	}
	for(int i = 0; i < PL_MAX_CONSTRAINT_BODIES; ++i)
		if(_bodies[i] == body)
		{
			_bodies[i] = NULL;
			_bodies[i]->removeConstraint(this);
		}
}

void PlConstraint::addToWorld()
{
	if(_world && _constraint && !_inWorld)
		_world->getWorld()->addConstraint(_constraint, _collidable);
}

void PlConstraint::removeFromWorld()
{
	if(_world && _constraint && _inWorld)
	{
		_world->getWorld()->removeConstraint(_constraint);
	}
	//ehn
}

void PlConstraint::setVector(int slot, btVector3& vec)
{
	if(slot < 0)
	{
		LOG(logERROR) << "Negative slot for PlConstraint::setVector!; got slotid: " << slot;
		return;
	}
	if(slot >= PL_MAX_CONSTRAINT_VECS)
	{
		LOG(logERROR) << "Too large slot for PlConstraint::setVector!; got slotid: " << slot << "\n"
					  << "(max slot id is: " << (PL_MAX_CONSTRAINT_VECS - 1);
		return;
	}
	_vecs[slot] = vec;
}

void PlConstraint::setQuaternion(int slot, btQuaternion& quat)
{
	if(slot < 0)
	{
		LOG(logERROR) << "Negative slot for PlConstraint::setQuaternion!; got slotid: " << slot;
		return;
	}
	if(slot >= PL_MAX_CONSTRAINT_QUATS)
	{
		LOG(logERROR) << "Too large slot for PlConstraint::setQuaternion!; got slotid: " << slot << "\n"
					  << "(max slot id is: " << (PL_MAX_CONSTRAINT_QUATS - 1);
		return;
	}
	_quats[slot] = quat;
}

void PlConstraint::addBody(int slot, PlRigidBody* body)
{
	if(slot < 0)
	{
		LOG(logERROR) << "Negative slot for PlConstraint::addBody!; got slotid: " << slot;
		return;
	}
	if(slot >= PL_MAX_CONSTRAINT_BODIES)
	{
		LOG(logERROR) << "Too large slot for PlConstraint::addBody!; got slotid: " << slot << "\n"
					  << "(max slot id is: " << (PL_MAX_CONSTRAINT_BODIES - 1);
		return;
	}
	if(_bodies[slot] != NULL)
	{
		_bodies[slot]->removeConstraint(this);
	}
	_bodies[slot] = body;
	body->addConstraint(this);
}

void PlConstraint::allowCollisions(bool state)
{
	_collidable = state;
}

void PlConstraint::setMotor(int idx, float targetVel, float maxForce)
{
        if(_constraint == NULL)
        {
                LOG(logERROR) << "Tried to set motor for uncreated constraint.";
                return;
        }

        switch(_conType)
        {
        case CON_HINGE:
			if(idx != 0)
				LOG(logWARNING) << "Hinge joint only has one motor; ignoring idx parameter (idx = " << idx << ")";
            ((btHingeConstraint*)(_constraint))->enableAngularMotor(true, targetVel, maxForce);
            break;
        case CON_LINEAR:
			if(idx == 0)
			{
                ((btSliderConstraint*)(_constraint))->setMaxLinMotorForce(maxForce);
                ((btSliderConstraint*)(_constraint))->setTargetLinMotorVelocity(targetVel);
                ((btSliderConstraint*)(_constraint))->setPoweredLinMotor(true);
			}
			else if(idx == 1)
			{
				((btSliderConstraint*)(_constraint))->setMaxAngMotorForce(maxForce);
                ((btSliderConstraint*)(_constraint))->setTargetAngMotorVelocity(targetVel);
                ((btSliderConstraint*)(_constraint))->setPoweredAngMotor(true);
			}
			else
				LOG(logERROR) << "Unknown motor idx for linear constraint; idx= " << idx;
			break;
        default:
                LOG(logERROR) << "Tried to set motor for unsupported constraint type.";
        }
}

void PlConstraint::disableMotor(int idx)
{
        if(_constraint == NULL)
        {
                LOG(logERROR) << "Tried to disable motor for uncreated constraint.";
                return;
        }

        switch(_conType)
        {
        case CON_HINGE:
			if(idx != 0)
				LOG(logWARNING) << "Hinge joint only has one motor; ignoring idx parameter (idx = " << idx << ")";
            ((btHingeConstraint*)(_constraint))->enableAngularMotor(false, 0.0, 0.0);
            break;
        case CON_LINEAR:
			if(idx == 0)
                ((btSliderConstraint*)(_constraint))->setPoweredLinMotor(false);
			else if(idx == 1)
				((btSliderConstraint*)(_constraint))->setPoweredAngMotor(false);
			else
				LOG(logERROR) << "Unknown motor idx for linear constraint; idx= " << idx;
            break;
        default:
                LOG(logERROR) << "Tried to disable motor for unsupported constraint type.";
        }
}

void PlConstraint::setLimits(int idx, float low, float high)
{
        if(_constraint == NULL)
        {
                LOG(logERROR) << "Tried to set limits for uncreated constraint.";
                return;
        }

        switch(_conType)
        {
        case CON_HINGE:
			if(idx != 0)
				LOG(logWARNING) << "Hinge joint only has one motor; ignoring idx parameter (idx = " << idx << ")";
            ((btHingeConstraint*)(_constraint))->setLimit(low, high);
            break;
        case CON_LINEAR:
			if(idx == 0)
			{
                ((btSliderConstraint*)(_constraint))->setLowerLinLimit(low);
                ((btSliderConstraint*)(_constraint))->setUpperLinLimit(high);
			}
			else if(idx == 1)
			{
				((btSliderConstraint*)(_constraint))->setLowerAngLimit(low);
                ((btSliderConstraint*)(_constraint))->setUpperAngLimit(high);
			}
			else
				LOG(logERROR) << "Unknown motor idx for linear constraint; idx= " << idx;
            break;
        default:
                LOG(logERROR) << "Tried to disable motor for unsupported constraint type.";
        }
}

int PlConstraint::numMotors()
{
	if(_constraint == NULL)
	{
		LOG(logWARNING) << "Warning: getting number of motors for uncreated constraint.";
	}

	switch(_conType)
	{
	case CON_LINEAR:
		return 2;
	case CON_HINGE:
		return 1;
	case CON_P2P:
		return 0;
	case CON_DEAD:
		return -1;
	case CON_NONE:
		return -2;
	default:
		LOG(logERROR) << "Unknown constraint type " << _conType;
		return 0;
	}
}

void PlConstraint::createHinge()
{

		if(_bodies[0] == NULL || _bodies[1] == NULL)
		{
			LOG(logERROR) << "Bodies not set!";
			return;
		}

        btRigidBody* bA = _bodies[0]->getBtBody();
        btRigidBody* bB = _bodies[1]->getBtBody();

        if(bA == NULL || bB == NULL)
        {
                LOG(logERROR) << "ConstraintAddon: BodyAddon exists but has no actual body!";
				return;
        }

		btVector3& localPosInA = _vecs[0];
		btVector3& localPosInB = _vecs[1];
		btVector3& localAxisInA = _vecs[2];
		btVector3& localAxisInB = _vecs[3];

        //LOG(logDEBUG2) << "PosInA: " << localPosInA.x() << localPosInA.y() << localPosInA.z();
        //LOG(logDEBUG2) << "AxisInA: " << localAxisInA.x() << localAxisInA.y() << localAxisInA.z();
        //LOG(logDEBUG2) << "PosInB: " << localPosInB.x() << localPosInB.y() << localPosInB.z();
        //LOG(logDEBUG2) << "AxisInB: " << localAxisInB.x() << localAxisInB.y() << localAxisInB.z();

        btDiscreteDynamicsWorld* pw = _world->getWorld();

        _constraint = new btHingeConstraint(*bA, *bB, localPosInA, localPosInB, localAxisInA, localAxisInB, false);
        _conType = CON_HINGE;
        LOG(logDEBUG2) << "Allow collide? " << _collidable;
		addToWorld();
        //pw->addConstraint(_constraint, _collidable);

        LOG(logDEBUG2) << "Made hinge constraint.";
}

void PlConstraint::createP2P()
{
        //if(!checkNotCreated())
        //        return;

        //if(!getAndCheckBodyRefs())
        //        return;
		if(_bodies[0] == NULL || _bodies[1] == NULL)
		{
			LOG(logERROR) << "Bodies not set!";
			return;
		}

        btRigidBody* bA = _bodies[0]->getBtBody();
        btRigidBody* bB = _bodies[1]->getBtBody();

        if(bA == NULL || bB == NULL)
        {
                LOG(logERROR) << "PlConstraint: Bodies set but have no actual bodies (maybe you forgot to finalize them?)";
				return;
        }

        btDiscreteDynamicsWorld* pw = _world->getWorld();
		btVector3& localPosInA = _vecs[0];
		btVector3& localPosInB = _vecs[1];

        _constraint = new btPoint2PointConstraint(*bA, *bB, localPosInA, localPosInB);
        _conType = CON_P2P;
		addToWorld();
        //pw->addConstraint(_constraint, _collidable);
}

void PlConstraint::createSlider()
{
		if(_bodies[0] == NULL || _bodies[1] == NULL)
		{
			LOG(logERROR) << "Bodies not set!";
			return;
		}

        btRigidBody* bA = _bodies[0]->getBtBody();
        btRigidBody* bB = _bodies[1]->getBtBody();

        if(bA == NULL || bB == NULL)
        {
                LOG(logERROR) << "PlConstraint: Bodies set but have no actual bodies (maybe you forgot to finalize them?)";
				return;
        }

        btDiscreteDynamicsWorld* pw = _world->getWorld();
		btVector3& transInA = _vecs[0];
		btVector3& transInB = _vecs[1];
		btQuaternion& rotInA = _quats[0];
		btQuaternion& rotInB = _quats[1];

        btTransform tA(rotInA, transInA);
        btTransform tB(rotInB, transInB);

        _constraint = new btSliderConstraint(*bA, *bB, tA, tB, false);
        _conType = CON_LINEAR;
		addToWorld();
        //pw->addConstraint(_constraint, _collidable);
}

void PlConstraint::createConeTwist()
{
	//TODO
	LOG(logERROR) << "PlConstraint::createConeTwist not implemented yet!";
}

PlShape::PlShape(PlBullet* world, int slotid, std::string name)
{
	_stub = new GeneroStub<PlShape>;
	_stub->refct = 1;
	_stub->target = this;
	_world = world;
	_slotid = slotid;
	_name = name;
	_finalized = false;
	_refct = 0;
	_shape = NULL;
	_isCompound = false;
}

PlShape::~PlShape()
{
	_stub->target = NULL;
	--_stub->refct;
	if(_stub->refct <= 0)
		delete _stub;
	delete _shape;
}

btCollisionShape* PlShape::getBtShape()
{
	return _shape;
}

int PlShape::getSlotId()
{
	return _slotid;
}

std::string PlShape::getName()
{
	return _name;
}

void PlShape::incrRefCt()
{
	++_refct;
}

void PlShape::decrRefCt()
{
	--_refct;
}

int PlShape::getRefCt()
{
	return _refct;
}

void PlShape::finalize()
{
	_finalized = true;
}

void PlShape::createBoxShape(double hwx, double hwy, double hwz)
{
	if(_finalized)
	{
		LOG(logERROR) << "Cannot create as box; already finalized";
		return;
	}
	_shape = new btBoxShape(btVector3((btScalar)hwx, (btScalar)hwy, (btScalar)hwz));
	finalize();
	_isCompound = false;
}

void PlShape::createSphereShape(double rad)
{
	if(_finalized)
	{
		LOG(logERROR) << "Cannot create as sphere; already finalized";
		return;
	}
	_shape = new btSphereShape((btScalar)rad);
	finalize();
	_isCompound = false;
}

void PlShape::createCylinderShape(double hwx, double hwy, double hwz)
{
	if(_finalized)
	{
		LOG(logERROR) << "Cannot create as cylinder; already finalized";
		return;
	}
	_shape = new btCylinderShape(btVector3((btScalar)hwx, (btScalar)hwy, (btScalar)hwz));
	finalize();
	_isCompound = false;
}

void PlShape::createCapsuleShape(double rad, double h)
{
	if(_finalized)
	{
		LOG(logERROR) << "Cannot create as capsule; already finalized";
		return;
	}
	_shape = new btCapsuleShape((btScalar)rad, (btScalar)h);
	finalize();
	_isCompound = false;
}

void PlShape::createConeShape(double rad, double h)
{
	if(_finalized)
	{
		LOG(logERROR) << "Cannot create as cone; already finalized";
		return;
	}
	_shape = new btConeShape((btScalar)rad, (btScalar)h);
	finalize();
	_isCompound = false;
}

void PlShape::createCompoundShape()
{
	if(_finalized)
	{
		LOG(logERROR) << "Cannot create as compound shape; already finalized";
		return;
	}
	_shape = new btCompoundShape;
	_isCompound = true;
}

void PlShape::addSubShape(PlShape* shap, btVector3& pos, btQuaternion& rot, float scale)
{
	if(_finalized || !_isCompound)
	{
		LOG(logERROR) << "Cannot add subshape; already finalized or not compound";
		return;
	}
	if(shap == NULL)
	{
		LOG(logERROR) << "Cannot add subshape: null subshape.";
		return;
	}
	if(shap->getBtShape() == NULL || !shap->_finalized)
	{
		LOG(logERROR) << "Cannot add subshape: subshape " << shap->getName() << " is not finalized.";
		return;
	}
	btTransform tempTForm(rot, pos);

	btCompoundShape* cs = (btCompoundShape*)_shape;
	cs->addChildShape(tempTForm, shap->getBtShape());
}

PlBullet::PlBullet()
{
	_world = NULL;
	_autoUpdating = false;
	_timestep = 1.0f / 60.0f;
	_maxsubsteps = 10;
}

PlBullet::~PlBullet()
{
	if(_world)
	{
		//TODO: bullet cleanup
	}
}

// stuff for creating joints and whatnot
PlRigidBody* PlBullet::createRigidBody()
{
	int idx = _bodies.nextFreeSlot();
	PlRigidBody* nb = new PlRigidBody(this, idx);
	_bodies.add(nb);
	return nb;
}

PlShape* PlBullet::createShape(std::string name)
{
	if(_shapeMap.count(name) > 0)
	{
		LOG(logERROR) << "Cannot create shape with name " << name << "; name taken.";
		return NULL;
	}
	int idx = _shapes.nextFreeSlot();
	PlShape* ns = new PlShape(this, idx, name);
	_shapeMap[name] = ns;
	_shapes.add(ns);
	return ns;
}

PlConstraint* PlBullet::createConstraint()
{
	int idx = _constraints.nextFreeSlot();
	PlConstraint* cp = new PlConstraint(this, idx);
	_constraints.add(cp);
	return cp;
}

PlRigidBody* PlBullet::getBody(int id)
{
	return _bodies[id];
}

PlShape* PlBullet::getShape(std::string name)
{
	if(_shapeMap.count(name) > 0)
		return _shapeMap[name];
	else
		return NULL;
}

PlConstraint* PlBullet::getConstraint(int id)
{
	return _constraints[id];
}

void PlBullet::deleteBody(PlRigidBody* body)
{
	if(body == NULL || body->getWorld() != this)
	{
		LOG(logERROR) << "Cannot delete body (null or wrong world).";
		return;
	}
	_bodies.remove(body->getSlotId());
	delete body;
}

void PlBullet::deleteConstraint(PlConstraint* constraint)
{
	if(constraint == NULL || constraint->getWorld() != this)
	{
		LOG(logERROR) << "Cannot delete constraint...";
		return;
	}
}

void PlBullet::deleteShape(PlShape* shape)
{
	if(shape == NULL || _shapeMap.count(shape->getName()) != 1 || _shapeMap[shape->getName()] != shape)
	{
		LOG(logERROR) << "Cannot delete shape (who knows why...)";
		return;
	}
	if(shape->getRefCt() > 0)
	{
		LOG(logERROR) << "Cannot delete shape; has " << shape->getRefCt() << " references.";
		return;
	}
}

void PlBullet::initPhysics()
{
	if(!_world)
	{
    btVector3 worldAabbMin(-10000,-10000,-10000);
    btVector3 worldAabbMax(10000,10000,10000);

	// note: defaults to a maximum of 16384 objects (it seems to represent w/ unsigned short, so this may well be the max)
    btAxisSweep3* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax);
    btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
    btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
    btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;

	 _world = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration);
	}
	else
		LOG(logWARNING) << "Physics already initted! (initting again has no effect)";
}

void PlBullet::setGravity(btVector3& grav)
{
	if(_world)
	{
		_world->setGravity(grav);
	}
	else
		LOG(logERROR) << "Cannot set gravity: physics not initted.";
}

void PlBullet::setInternalTimestep(double dt)
{
	_timestep = (float)dt;
}

void PlBullet::stepSimulation(double dt)
{
	if(_world)
		_world->stepSimulation((btScalar)dt, _maxsubsteps, (btScalar)_timestep);
}

void PlBullet::setMaxSubsteps(int maxSubsteps)
{
	_maxsubsteps = maxSubsteps;
}

void PlBullet::setAutoStep(bool autoStep)
{
	_autoUpdating = autoStep;
}

btVector3 PlBullet::makeVector()
{
	btVector3 ret(0.0,0.0,0.0);
	return ret;
}

btQuaternion PlBullet::makeQuaternion()
{
	btQuaternion ret;
	return ret;
}

btTransform PlBullet::makeTransform()
{
	btTransform ret;
	return ret;
}

btDiscreteDynamicsWorld* PlBullet::getWorld()
{
	return _world;
}

// General abstract object stuff goes below

// Return the c++ type id
 int PlBullet::getCppTypeId()
 {
	 //TODO: assign this some id
	 return 0;
 }
// Returns whether this can be cast to a given type
 bool PlBullet::isType(int id)
 {
	 return false;
 }

// Return the c++ type as a string 
 std::string PlBullet::getCppTypeName()
 {
	 return "PlBullet";
 }

// returns a description of this thing
 std::string PlBullet::getDescription()
 {
	 return "Bullet physics library component.";
 }

// create another one of these
 AbstractObject* PlBullet::newInstance()
 {
	 return new PlBullet;
 }

// init & update
 void PlBullet::init()
 {
	 // nothing special to do
 }

 void PlBullet::update(double dt)
 {
	 if(_autoUpdating && _world)
		stepSimulation(dt);
 }

// push its static functions into the given lua table
 void PlBullet::pushStaticFuncs(lua_State* L)
 {
	 plbtAOBindings::pushStaticFuncs(L);
	 // we could push custom functions after this, but we have none at the moment.
 }